/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-08-31 10:14:34
 * @LastEditors: EltonLi lijiaming@uavi-tech.com
 * @LastEditTime: 2024-07-08 17:21:38
 * @FilePath: \zino-fc-v4\ZINO\sensor\LSM6DSR\LSM6DSR.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-08-31 10:14:34
 * @LastEditors: EltonLi lijiaming@uavi-tech.com
 * @LastEditTime: 2023-09-07 11:21:37
 * @FilePath: \zino-fc-v4\ZINO\sensor\LSM6DSR\LSM6DSR.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "lsm6dsr.h"
#include "zino.h"
#include <drv_spi.h>
#include <rtthread.h>

#include <sensor.h>

#define DBG_TAG "LSM6DSR"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define LSM6DSR_DEVICE_NAME "spi_lsm"

static rt_err_t lsm6dsr_spi_init(lsm6dsr_device_t *dev)
{
    dev->spi_dev = (struct rt_spi_device *)rt_device_find(LSM6DSR_DEVICE_NAME);
    if (dev->spi_dev != RT_NULL)
    {
        dev->spi_dev->config.max_hz = 10000000;
        dev->spi_dev->config.data_width = 8;
        dev->spi_dev->config.mode = RT_SPI_MODE_3 | RT_SPI_MSB;
        LOG_D("%s spi init ok!", LSM6DSR_DEVICE_NAME);
        return RT_EOK;
    }
    else
    {
        LOG_E("init failed! can't find %s spi!", LSM6DSR_DEVICE_NAME);
        return -RT_ERROR;
    }
}

static rt_err_t lsm6dsr_spi_write_reg(lsm6dsr_device_t *dev, uint8_t reg_addr, uint8_t data)
{
    return rt_spi_send_then_send(dev->spi_dev, &reg_addr, 1, &data, 1);
}

static rt_err_t lsm6dsr_spi_write_regs(lsm6dsr_device_t *dev, uint8_t reg_addr, uint8_t *data, uint8_t len)
{
    return rt_spi_send_then_send(dev->spi_dev, &reg_addr, 1, data, len);
}

static rt_err_t lsm6dsr_spi_read_reg(lsm6dsr_device_t *dev, uint8_t reg_addr, uint8_t *data)
{
    reg_addr |= 0x80;
    return rt_spi_send_then_recv(dev->spi_dev, &reg_addr, 1, data, 1);
}

static rt_err_t lsm6dsr_spi_read_regs(lsm6dsr_device_t *dev, uint8_t reg_addr, uint8_t *data, uint8_t len)
{
    reg_addr |= 0x80;
    return rt_spi_send_then_recv(dev->spi_dev, &reg_addr, 1, data, len);
}

static rt_err_t lsm6dsr_spi_write_bits(lsm6dsr_device_t *dev, rt_uint8_t reg, rt_uint8_t start_bit, rt_uint8_t len, rt_uint8_t data)
{
    rt_uint8_t byte, mask;
    rt_err_t res;

    res = lsm6dsr_spi_read_regs(dev, reg, &byte, 1);
    if (res != RT_EOK)
    {
        return res;
    }
    mask = ((1 << len) - 1) << (start_bit - len + 1);
    data <<= (start_bit - len + 1); // shift data into correct position
    data &= mask;                   // zero all non-important bits in data
    byte &= ~(mask);                // zero all important bits in existing byte
    byte |= data;
    return lsm6dsr_spi_write_regs(dev, reg, &byte, 1);
}

rt_err_t lsm6dsr_spi_set_reg(lsm6dsr_device_t *dev, uint8_t cmd, uint8_t param)
{
    rt_err_t res = RT_EOK;
    switch (cmd)
    {
    case LSM6DSR_ACC_ODR_SET:
        res = lsm6dsr_spi_write_bits(dev, LSM6DSR_CTRL1_XL, 7, 4, param);
        break;
    case LSM6DSR_ACC_RANGE_SET:
        res = lsm6dsr_spi_write_bits(dev, LSM6DSR_CTRL1_XL, 3, 2, param);
        dev->range.acce_range = param;
        rt_kprintf("sen_acce = %d\n", dev->range.acce_range);
        break;
    case LSM6DSR_GYRO_LPF1_SET:
        res = lsm6dsr_spi_write_bits(dev, LSM6DSR_CTRL4_C, 1, 1, param);
        break;
    case LSM6DSR_LPF1_ODR_SET:
        res = lsm6dsr_spi_write_bits(dev, LSM6DSR_CTRL6_C, 2, 3, param);
        break;
    case LSM6DSR_GYRO_ODR_SET:
        res = lsm6dsr_spi_write_bits(dev, LSM6DSR_CTRL2_G, 7, 4, param);
        break;
    case LSM6DSR_GYRO_RANGE_SET:
        res = lsm6dsr_spi_write_bits(dev, LSM6DSR_CTRL2_G, 3, 4, param);
        dev->range.gyro_range = param;
        rt_kprintf("sen_gyro = %d\n", dev->range.gyro_range);
        break;
    case LSM6DSR_GHM_MODE_SET:
        res = lsm6dsr_spi_write_reg(dev, LSM6DSR_CTRL7_G, param);
        break;
    case LSM6DSR_XLHM_MODE_SET:
        res = lsm6dsr_spi_write_reg(dev, LSM6DSR_CTRL6_C, param);
        break;
    default:
        break;
    }
    return res;
}

rt_err_t lsm6dsr_init(lsm6dsr_device_t *dev, const char *bus_name)
{
    rt_uint8_t res = RT_EOK;
    res = lsm6dsr_spi_init(dev);
    uint8_t try=0;
    if (res != RT_EOK)
    {
        LOG_E("lsm6dsr find spi device failed");
        return -RT_ERROR;
    }
    while (1)
    {
        lsm6dsr_spi_read_reg(dev, LSM6DSR_WHO_AM_I, &dev->dev_id);
        if (dev->dev_id != LSM6DSR_ID)
        {
            LOG_E("LSM6DSR read id error, rx:0x%x, expect:0x%x", dev->dev_id, LSM6DSR_ID);
            // return -RT_ERROR;
            if(try++>3) return -RT_ERROR;
        }
        else
        {
            LOG_D("LSM6DSR read id success, rx:0x%x, expect:0x%x", dev->dev_id, LSM6DSR_ID);
            res += lsm6dsr_spi_set_reg(dev, LSM6DSR_ACC_RANGE_SET, LSM6DSR_4g);
            res += lsm6dsr_spi_set_reg(dev, LSM6DSR_ACC_ODR_SET, ACC_ODR_208HZ);
            res += lsm6dsr_spi_set_reg(dev, LSM6DSR_GYRO_RANGE_SET, LSM6DSR_2000dps);
            res += lsm6dsr_spi_set_reg(dev, LSM6DSR_GYRO_ODR_SET, GYRO_ODR_208HZ);
            // res += lsm6dsr_spi_set_reg(dev, LSM6DSR_DLPF_SET, GYRO_LPF1_EN);
            res += lsm6dsr_spi_set_reg(dev, LSM6DSR_GHM_MODE_SET, G_HM_MODE_ENABLE);
            res += lsm6dsr_spi_set_reg(dev, LSM6DSR_XLHM_MODE_SET, XL_HM_MODE_ENABLE);
            break;
        }
    }

    if (res == RT_EOK)
    {
        LOG_I("Device init succeed!");
    }
    else
    {
        LOG_W("Error in device initialization!");
    }
    return res;
}

rt_err_t lsm6dsr_get_acc_raw(lsm6dsr_device_t *dev)
{
    rt_err_t res;
    rt_uint8_t buffer[6];
    res = lsm6dsr_spi_read_regs(dev, LSM6DSR_OUTX_L_ACC, buffer, 6);
    if (res != RT_EOK)
        return res;
    dev->acce_raw.y = (buffer[1] << 8) | buffer[0];
    dev->acce_raw.x = (buffer[3] << 8) | buffer[2];
    dev->acce_raw.z = (buffer[5] << 8) | buffer[4];
    return RT_EOK;
}
rt_err_t lsm6dsr_get_gyro_raw(lsm6dsr_device_t *dev)
{
    rt_err_t res;
    rt_uint8_t buffer[6];
    res = lsm6dsr_spi_read_regs(dev, LSM6DSR_OUTX_L_GYRO, buffer, 6);
    if (res != RT_EOK)
        return res;
    dev->gyro_raw.y = (buffer[1] << 8) | buffer[0];
    dev->gyro_raw.x = (buffer[3] << 8) | buffer[2];
    dev->gyro_raw.z = (buffer[5] << 8) | buffer[4];
    return RT_EOK;
}

rt_err_t lsm6dsr_get_acc(lsm6dsr_device_t *dev)
{
    rt_uint16_t sen;
    rt_err_t res;

    res = lsm6dsr_get_acc_raw(dev);
    if (res != RT_EOK)
        return res;
    sen = dev->range.acce_range;

    LOG_D("acc: %04d \t %04d \t %04d", dev->acce_raw.x, dev->acce_raw.y, dev->acce_raw.z);
    switch (sen)
    {
    case LSM6DSR_2g:
        dev->acce.x = dev->acce_raw.x * 0.061f;
        dev->acce.y = dev->acce_raw.y * 0.061f;
        dev->acce.z = dev->acce_raw.z * 0.061f;
        break;
    case LSM6DSR_4g:
        dev->acce.x = dev->acce_raw.x * 0.122f;
        dev->acce.y = dev->acce_raw.y * 0.122f;
        dev->acce.z = dev->acce_raw.z * 0.122f;
        break;
    case LSM6DSR_8g:
        dev->acce.x = dev->acce_raw.x * 0.244f;
        dev->acce.y = dev->acce_raw.y * 0.244f;
        dev->acce.z = dev->acce_raw.z * 0.244f;
        break;
    case LSM6DSR_16g:
        dev->acce.x = dev->acce_raw.x * 0.488f;
        dev->acce.y = dev->acce_raw.y * 0.488f;
        dev->acce.z = dev->acce_raw.z * 0.488f;
        break;
    default:
        break;
    }
    return RT_EOK;
}
rt_err_t lsm6dsr_get_gyro(lsm6dsr_device_t *dev)
{
    rt_uint16_t sen;
    rt_err_t res;

    res = lsm6dsr_get_gyro_raw(dev);
    sen = dev->range.gyro_range;
    if (res != RT_EOK)
        return res;
    switch (sen)
    {
    case LSM6DSR_125dps:
        dev->gyro.x = dev->gyro_raw.x * 4.375f * 1000;
        dev->gyro.y = dev->gyro_raw.y * 4.375f * 1000;
        dev->gyro.z = dev->gyro_raw.z * 4.375f * 1000;
        break;
    case LSM6DSR_250dps:
        dev->gyro.x = dev->gyro_raw.x * 8.750f * 1000;
        dev->gyro.y = dev->gyro_raw.y * 8.750f * 1000;
        dev->gyro.z = dev->gyro_raw.z * 8.750f * 1000;
        break;
    case LSM6DSR_500dps:
        dev->gyro.x = dev->gyro_raw.x * 17.50f * 1000;
        dev->gyro.y = dev->gyro_raw.y * 17.50f * 1000;
        dev->gyro.z = dev->gyro_raw.z * 17.50f * 1000;
        break;
    case LSM6DSR_1000dps:
        dev->gyro.x = dev->gyro_raw.x * 35.0f * 1000;
        dev->gyro.y = dev->gyro_raw.y * 35.0f * 1000;
        dev->gyro.z = dev->gyro_raw.z * 35.0f * 1000;
        break;
    case LSM6DSR_2000dps:
        dev->gyro.x = dev->gyro_raw.x * 70.0f * 1000;
        dev->gyro.y = dev->gyro_raw.y * 70.0f * 1000;
        dev->gyro.z = dev->gyro_raw.z * 70.0f * 1000;
        break;
    case LSM6DSR_4000dps:
        dev->gyro.x = dev->gyro_raw.x * 140.0f * 1000;
        dev->gyro.y = dev->gyro_raw.y * 140.0f * 1000;
        dev->gyro.z = dev->gyro_raw.z * 140.0f * 1000;
        break;

    default:
        break;
    }
    return RT_EOK;
}

lsm6dsr_device_t *_lsm6dsr_init(struct rt_sensor_intf *intf)
{
    lsm6dsr_device_t *lsm6dsr = RT_NULL;
    lsm6dsr = rt_malloc(sizeof(lsm6dsr_device_t));
    if (lsm6dsr == RT_NULL)
        return RT_NULL;
    if(lsm6dsr_init(lsm6dsr, intf->dev_name)==RT_EOK)
    return lsm6dsr;
    else
        return RT_NULL;
}

static rt_size_t lsm6dsr_fetch_data(struct rt_sensor_device *sensor, void *buf, rt_size_t len)
{
    lsm6dsr_device_t *LSM6DSR = sensor->parent.user_data;
    struct rt_sensor_data *data = (struct rt_sensor_data *)buf;
    if (sensor->info.type == RT_SENSOR_CLASS_ACCE)
    {
        if (lsm6dsr_get_acc_raw(LSM6DSR) != RT_EOK)
            return 0;
        data->type = RT_SENSOR_CLASS_ACCE;
        data->data.acce.x = -LSM6DSR->acce_raw.x;
        data->data.acce.y = -LSM6DSR->acce_raw.y;
        data->data.acce.z = -LSM6DSR->acce_raw.z;
        data->timestamp = rt_sensor_get_ts();
        data->range = LSM6DSR->range.acce_range;
    }
    else if (sensor->info.type == RT_SENSOR_CLASS_GYRO)
    {
        if (lsm6dsr_get_gyro_raw(LSM6DSR) != RT_EOK)
            return 0;
        data->type = RT_SENSOR_CLASS_GYRO;
        data->data.gyro.x = -LSM6DSR->gyro_raw.x;
        data->data.gyro.y = -LSM6DSR->gyro_raw.y;
        data->data.gyro.z = -LSM6DSR->gyro_raw.z;
        data->timestamp = rt_sensor_get_ts();
        data->range = LSM6DSR->range.gyro_range;
    }
    return RT_EOK;
}

static rt_err_t lsm6dsr_control(struct rt_sensor_device *sensor, int cmd, void *args)
{
    rt_err_t res = RT_EOK;
    lsm6dsr_device_t *LSM6DSR = sensor->parent.user_data;
    switch (cmd)
    {
    case RT_SENSOR_CTRL_SET_ODR:
        if (sensor->info.type == RT_SENSOR_CLASS_ACCE)
        {
            res = lsm6dsr_spi_set_reg(LSM6DSR, LSM6DSR_ACC_ODR_SET, (rt_uint32_t)args);
        }
        else if (sensor->info.type == RT_SENSOR_CLASS_GYRO)
        {
            res = lsm6dsr_spi_set_reg(LSM6DSR, LSM6DSR_GYRO_ODR_SET, (rt_uint32_t)args);
        }
        break;
    case RT_SENSOR_CTRL_SET_RANGE:
        if (sensor->info.type == RT_SENSOR_CLASS_ACCE)
        {
            res = lsm6dsr_spi_set_reg(LSM6DSR, LSM6DSR_ACC_RANGE_SET, (rt_uint32_t)args);
        }
        else if (sensor->info.type == RT_SENSOR_CLASS_GYRO)
        {
            res = lsm6dsr_spi_set_reg(LSM6DSR, LSM6DSR_GYRO_RANGE_SET, (rt_uint32_t)args);
        }
        break;
    default:
        break;
    }
    return res;
}

static struct rt_sensor_ops lsm6dsr_ops =
    {
        lsm6dsr_fetch_data,
        lsm6dsr_control};

int rt_hw_lsm6dsr_init(const char *name, struct rt_sensor_config *cfg)
{
    rt_uint8_t res = RT_EOK;
    rt_sensor_t lsm_acce = RT_NULL, lsm_gyro = RT_NULL;
    lsm6dsr_device_t *LSM6DSR = _lsm6dsr_init(&cfg->intf);
    if (LSM6DSR == RT_NULL)
    {
        LOG_E("_lsm6dsr init err!");
        return -RT_ERROR;
    }
    lsm_acce = rt_calloc(1, sizeof(struct rt_sensor_device));
    if (lsm_acce == RT_NULL)
        return -RT_ERROR;
    lsm_acce->info.type = RT_SENSOR_CLASS_ACCE;
    lsm_acce->info.vendor = RT_SENSOR_VENDOR_STM;
    lsm_acce->info.model = "lsm6dsr_acc";
    lsm_acce->info.unit = RT_SENSOR_UNIT_MG;
    lsm_acce->info.intf_type = RT_SENSOR_INTF_SPI;
    lsm_acce->info.period_min = 10;
    lsm_acce->info.fifo_max = 0;

    rt_memcpy(&lsm_acce->config, cfg, sizeof(struct rt_sensor_config));
    lsm_acce->ops = &lsm6dsr_ops;
    res = rt_hw_sensor_register(lsm_acce, name, RT_DEVICE_FLAG_RDWR, LSM6DSR);
    if (res != RT_EOK)
    {
        LOG_E("device register err code: %d", res);
        rt_free(lsm_acce);
        return -RT_ERROR;
    }
    LOG_I("lsm6dsr acc device init success");

    lsm_gyro = rt_calloc(1, sizeof(struct rt_sensor_device));
    if (lsm_gyro == RT_NULL)
        return -RT_ERROR;

    lsm_gyro->info.type = RT_SENSOR_CLASS_GYRO;
    lsm_gyro->info.vendor = RT_SENSOR_VENDOR_STM;
    lsm_gyro->info.model = "lsm6dsr_gyro";
    lsm_gyro->info.unit = RT_SENSOR_UNIT_MDPS;
    lsm_gyro->info.intf_type = RT_SENSOR_INTF_SPI;
    lsm_gyro->info.period_min = 10;
    lsm_gyro->info.fifo_max = 0;
    rt_memcpy(&lsm_gyro->config, cfg, sizeof(struct rt_sensor_config));
    lsm_gyro->ops = &lsm6dsr_ops;
    res = rt_hw_sensor_register(lsm_gyro, name, RT_DEVICE_FLAG_RDWR, LSM6DSR);
    if (res != RT_EOK)
    {
        LOG_E("device register err code: %d", res);
        rt_free(lsm_gyro);
        return -RT_ERROR;
    }
    LOG_I("lsm6dsr gyro device success");
    return RT_EOK;
}

int rt_hw_lsm6dsr_port(void)
{
    struct rt_sensor_config cfg;
    // cfg.intf.dev_name = "spi1";
    cfg.irq_pin.pin = RT_PIN_NONE;
    return rt_hw_lsm6dsr_init("lsm", &cfg);
}

// MSH_CMD_EXPORT(rt_hw_lsm6dsr_port,init lsm);

void rt_hw_lsm6dsr_data(void)
{
    rt_device_t lsm_acce = RT_NULL, lsm_gyro = RT_NULL;
    struct rt_sensor_data lsm_acce_data, lsm_gyro_data;

    lsm_acce = rt_device_find("acce_lsm");
    lsm_gyro = rt_device_find("gyro_lsm");
    rt_device_open(lsm_acce, RT_DEVICE_FLAG_RDWR);
    rt_device_open(lsm_gyro, RT_DEVICE_FLAG_RDWR);
    rt_device_control(lsm_acce, RT_SENSOR_CTRL_SET_RANGE, (void *)LSM6DSR_2g);
    rt_device_control(lsm_gyro, RT_SENSOR_CTRL_SET_RANGE, (void *)LSM6DSR_2000dps);
    rt_device_control(lsm_acce, RT_SENSOR_CTRL_SET_ODR, (void *)ACC_ODR_208HZ);
    rt_device_control(lsm_gyro, RT_SENSOR_CTRL_SET_ODR, (void *)GYRO_ODR_208HZ);
    while (1)
    {
        rt_device_read(lsm_acce, 0, &lsm_acce_data, 1);
        rt_device_read(lsm_gyro, 0, &lsm_gyro_data, 1);
        // rt_kprintf("acce: x:%5d, y:%5d, z:%5d \t", lsm_acce_data.data.acce.x, lsm_acce_data.data.acce.y, lsm_acce_data.data.acce.z);
        rt_kprintf("gyro: x:%5d, y:%5d, z:%5d, timestamp:%5d\n", lsm_gyro_data.data.gyro.x, lsm_gyro_data.data.gyro.y, lsm_gyro_data.data.gyro.z, lsm_gyro_data.timestamp);
        rt_thread_mdelay(10);
    }
}

ZINO_APP_EXPORT(rt_hw_lsm6dsr_port);

// MSH_CMD_EXPORT(rt_hw_lsm6dsr_data, lsm6dsr data);
